'''
Author: newsun
Date: 2022-01-07 20:30:34
LastEditTime: 2022-01-14 17:03:54
LastEditors: newsun-HP-Pavilion-Gaming-Laptop-15-dk0xxx
Description: 前哨站下装甲击打
FilePath: /Vision_bak/tool/curve_plot/anti-spin.py
'''

#已知前哨站下装甲旋转速度为0.4r/s(每秒转0.4圈)

import numpy as np
import matplotlib.pyplot as plt
from sys import argv
import time

from pandas import period_range
def procedure():
    time.sleep(0.045)
if __name__ == "__main__":
    sampling_num = 30
    boundary = 320
    data_r = np.loadtxt("tool/curve_plot/data_robust.txt", delimiter=',')
    data = [[0,0]]
    t0 = time.time()
    plt.ion()  #打开交互模式
    for line in range(data_r.shape[0]):
        procedure()
        is_spin = 1#是否在陀螺
        k = 0
        ct = time.time() - t0
        data = np.append(data,[[data_r[line,0],ct]],axis = 0)
        if line < 60: 
            continue
        data = np.delete(data,0,axis = 0)
        print("ct:",ct)
        data_0 = data[:,0].copy()
        data_v = data_0.copy()
        v_points = []#2i为跳变点的index，2i+1为差分值
        for index in range(1,len(data_0)):
            data_v[index] = data_0[index] - data_0[index-1]
            if(abs(data_v[index]) > 150): 
                v_points.append(index)
                v_points.append(data_v[index])
        # print(v_points)
        if len(v_points) < 6:  ## 跳变点太少的不要
            is_spin = 0
            # continue
        for i in range(2,len(v_points),2):
            if abs(v_points[i + 1] - v_points[i - 1]) > 80 or abs(v_points[i] - v_points[i - 2]) > 20 \
                or abs(v_points[i] - v_points[i - 2]) < 3 :#差分值差距不能超过80，相邻跳变点相隔不能超过20
                is_spin = 0
                # continue
        
        if is_spin == 1 :
            print(len(v_points))
            period_T = 0
            ks = 0
            # 估算周期T和k
            for i in range(2,len(v_points),2):
                period_T = period_T + data[v_points[i],1] - data[v_points[i - 2],1]
                ks = ks + data[v_points[i]-1,0] - data[v_points[i - 2],0]/(data[v_points[i]-1,1] - data[v_points[i - 2],1])
            period = (period_T / (len(v_points)/2 - 1))*3
            print(period)
            k = ks / ((len(v_points)-1)/2)
            omega = 2*np.pi/period
            hz = 1/period
            #假设估算0.2s以后
            #delta_t
            delta_t = (data_r[line,0]-data[v_points[-2],0])/k
            dt = (ct-data[v_points[-2],1])
            print(delta_t)
            print(dt)
            
        
        
        
        # print(mid_avg)
        ## 选取最后一次递增为击打中心点
        #动态画图
        # plt.figure(0)
        # plt.title('x_position')
        # if is_spin == 1:
        #     plt.scatter(np.arange(len(data_0)),data_0,c = 'r')
        # else:
        #     plt.scatter(np.arange(len(data_0)),data_0,c = 'b')
        # plt.show()
        # plt.pause(0.001)
        # plt.clf()  #清除图像
